# coding: utf-8

from __future__ import print_function
import can
import time
import keyboard
import csv
from enum import Enum

'''版本：1.2
        2022-01-14'''


class CarState(Enum):
    standby = 0
    forward_acc20 = 1
    forward_acc40 = 2
    reverse = 3
    reverse_acc15 = 4
    start = 5

class CarController():

    def __init__(self, *, bitrate=500_000):
        # self.steerRange = [-633.5, 595.3]
        self.steerRange = [-650, 650]
        self.selfDriving_req = 1
        self.stopCar_req = 0
        # self.bus0 = can.interface.Bus(bustype='socketcan', channel='can0', bitrate=bitrate)
        self.bus0 = can.interface.Bus(bustype='canalystii', channel=0, bitrate=bitrate)  # 初始化CAN0通道用来发送

        self.currentAcc = 0  # 用于记录当前acc、brake、steer
        self.currentBrake = 0
        self.currentSteer = 0
        self.currentGearMod = 0

        self.accSlow = 20
        self.accFast = 40
        self.accBack = 15

    def recv(self):
        bus = self.bus0
        try:
            msg = bus.recv(timeout=5)  # 接收信息
            if msg != None:
                # print('Message received on {}'.format(bus.channel_info))
                # print(msg.data)

                msg_data = msg.data
                timestamp = time.time()
                vcu_speed, vcu_acc, eps_steer, vcu_gear = 0, 0, 0, 0
                '''节点:VCU，id:0x18F101D0
                   车速:vcu_speed    档位:vcu_gear     加速踏板开度:vcu_acc
                   节点:EPS，id:0x0165     转角反馈:eps_steer'''
                if msg.arbitration_id == 0x0165:  # 转角反馈，EPS节点0x165
                    eps_steer = (msg_data[1] << 8) + msg_data[2]
                elif msg.arbitration_id == 0x18F101D0:
                    vcu_speed = ((msg_data[1] << 8) + msg_data[2]) / 256  # unit: km/h
                    vcu_acc = msg_data[6]  # unit: %
                    vcu_gear = msg_data[3] & 0x03  # 0：空挡；1：前进；2：倒挡

                return vcu_speed, vcu_acc, eps_steer, vcu_gear, timestamp
        except can.CanError:
            print('Message receiving timeout')

    def cansend_one(self, can_data, *, arbitration_id=0x0C01D0E1, is_extended_id=True):
        msg = can.Message(arbitration_id=arbitration_id,
                          data=can_data,
                          is_extended_id=is_extended_id)
        try:
            self.bus0.send(msg)
            print("Message sent on {}".format(self.bus0.channel_info))
        except can.CanError:
            print("Message NOT sent")

    def move(self, acc: int, brake: int, steer: float, gearMod: int) -> None:
        can_data = self.gen_candata(acc, brake, steer, gearMod)
        self.cansend_one(can_data)
        self.set_current(acc, brake, steer, gearMod)
        print(f'current stade: acc={acc}, brake={brake}, steer={steer}, gearMod={gearMod}')

    def gear(self, gearMod: int, *, acc: int = 0, brake: int = 0, steer: float = 0.) -> None:
        '''0：空挡；1：前进；2：倒挡'''
        can_data = self.gen_candata(acc, brake, steer, gearMod)
        self.cansend_one(can_data)
        self.set_current(acc, brake, steer, gearMod)

    def accelerate(self, acc: int, *, brake: int = 0, steer: float = 0., gearMod: int = None) -> None:
        if gearMod is None:
            gearMod = self.currentGearMod

        can_data = self.gen_candata(acc, brake, steer, gearMod)
        self.cansend_one(can_data)
        self.set_current(acc, brake, steer, gearMod)

    def brake(self, brake: int, *, acc: int = 0, steer: float = 0., gearMod: int = None) -> None:
        if gearMod is None:
            gearMod = self.currentGearMod

        can_data = self.gen_candata(acc, brake, steer, gearMod)
        self.cansend_one(can_data)
        self.set_current(acc, brake, steer, gearMod)

    def steer(self, steer: float, *, acc: int = 0, brake: int = 0, gearMod: int = None) -> None:
        if gearMod is None:
            gearMod = self.currentGearMod

        can_data = self.gen_candata(acc, brake, steer, gearMod)
        self.cansend_one(can_data)
        self.set_current(acc, brake, steer, gearMod)

    def gen_candata(self, acc: int, brake: int, steer: float, gearMod: int) -> list:
        '''首先判断数据是否超限'''
        if not ((0 <= acc <= 60) and (0 <= brake <= 60) and (-700 <= steer <= 700)):
            raise ValueError
        if not (gearMod == 0 or gearMod == 1 or gearMod == 2):
            raise ValueError

        steer_bus_value = int((steer + 870) * 10)
        steer_low = steer_bus_value & 0x00ff
        steer_high = steer_bus_value >> 8
        candata_5th_byte = (self.stopCar_req << 3) + (gearMod << 1) + self.selfDriving_req

        steeringAngle_speed_req = 0

        candata = [acc, brake, steer_low, steer_high, steeringAngle_speed_req, candata_5th_byte, 0, 0]
        return candata

    def set_current(self, acc, brake, steer, geerMod):
        self.currentAcc = acc
        self.currentBrake = brake
        self.currentSteer = steer
        self.currentGearMod = geerMod

    def startup(self) -> None:
        print("启动")
        # self.brake(30,gearMod=1)
        # self.move(brake=30,gearMod=1)




    def keyboard_ctl(self):  # 键盘控制行驶
        # ---------------------------((acc, brake, steer, geerMod),'Current_state')
        x_state = {(CarState.standby, 'w'):             ((self.accSlow, 0, 0, 1), CarState.forward_acc20),
                   (CarState.forward_acc20, 'W'):       ((self.accFast, 0, 0, 1), CarState.forward_acc40),
                   (CarState.standby, 'W'):             ((self.accFast, 0, 0, 1), CarState.forward_acc40),
                   (CarState.forward_acc40, 'w'):       ((self.accSlow, 0, 0, 1), CarState.forward_acc20),
                   (CarState.forward_acc20, 's'):       ((0, 20, 0, 1), CarState.standby),
                   (CarState.forward_acc20, 'space'):   ((0, 40, 0, 1), CarState.standby),
                   (CarState.forward_acc40, 'space'):   ((0, 40, 0, 1), CarState.standby),
                   (CarState.forward_acc40, 's'):       ((0, 20, 0, 1), CarState.standby),
                   (CarState.standby, 'r'):             ((0, 30, 0, 2), CarState.reverse),
                   (CarState.reverse_acc15, 'w'):       ((0, 20, 0, 2), CarState.reverse),
                   (CarState.reverse_acc15, 'space'):   ((0, 40, 0, 2), CarState.reverse),
                   (CarState.reverse, 's'):             ((self.accBack, 0, 0, 2), CarState.reverse_acc15),
                   (CarState.reverse, 'e'):             ((0, 30, 0, 1), CarState.standby)}

        current_state = CarState.standby
        current_steer = 0
        result = (0, 20, 0, 1)
        while True:
            key = keyboard.read_key()
            try:
                result, current_state = x_state[(current_state, key)]
            except Exception as e:
                print(e)
            print(current_state)
            if key == 'd' and current_steer > -600:
                current_steer -= 10
            elif key == 'a' and current_steer < 600:
                current_steer += 10
            elif key == 'z':
                current_steer = 0
            else:
                pass
            result = list(result)
            result[2] = current_steer

            # with open('./data/send.csv', 'a', newline='', encoding='utf-8') as csvfile:
            #     writer = csv.writer(csvfile, delimiter=' ')
            #     writer.writerow([result, int(time.time())])

            self.move(*result)


if __name__ == '__main__':
    tbox = CarController()
    #tbox.startup()
    tbox.keyboard_ctl()

